#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include "chrono"
#include "thread"

static float linear_vel = 1.0;
static float angular_vel = 0.5;
static float max_linear_vel = 2.0;
static float min_linear_vel = 0.0;
static float vel_increment = 0.1;

int GetCh()
{
  static struct termios oldt, newt;
  tcgetattr(STDIN_FILENO, &oldt);
  newt = oldt;
  newt.c_lflag &= ~(ICANON);
  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
  int c = getchar();
  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
  return c;
}
void setVelocity(geometry_msgs::msg::Twist &cmd, float linear, float angular)
{
  cmd.linear.x = linear;
  cmd.angular.z = angular;
  printf(" - linear.x= %.2f  angular.z= %.2f \n", cmd.linear.x, cmd.angular.z);
}
int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  printf("键盘控制机器人移动: \n");
  printf("w  向前 \n");
  printf("s  后退 \n");
  printf("a  左转 \n");
  printf("d  右转 \n");
  printf("=  加速 \n");
  printf("-  减速 \n");
  printf("space 停止 \n");
  printf("x  退出 \n");
  printf("------------- \n");

  auto node = rclcpp::Node::make_shared("keyboard_vel_cmd");

  auto cmd_vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

  auto base_cmd = std::make_shared<geometry_msgs::msg::Twist>();
  base_cmd->linear.x = 0;
  base_cmd->linear.y = 0;
  base_cmd->angular.z = 0;
  setVelocity(*base_cmd, 0, 0);
  while (rclcpp::ok())
  {
    int cKey = GetCh();
    printf("cKey = %c \n", cKey);
    if (cKey == 'w')
    {
      setVelocity(*base_cmd, linear_vel, 0);
    }
    else if (cKey == 's')
    {
      setVelocity(*base_cmd, -linear_vel, 0);
    }
    else if (cKey == 'a')
    {
      setVelocity(*base_cmd, 0, angular_vel);
    }
    else if (cKey == 'd')
    {
      setVelocity(*base_cmd, 0, -angular_vel);
    }
    else if (cKey == ' ')
    {
      setVelocity(*base_cmd, 0, 0);
    }
    else if (cKey == '-')
    {
      linear_vel -= vel_increment;
      if (linear_vel <= min_linear_vel)
      {
        linear_vel = min_linear_vel;
      }
      setVelocity(*base_cmd, linear_vel, 0);
    }
    else if (cKey == '=')
    {
      linear_vel += vel_increment;
      if (linear_vel >= max_linear_vel)
      {
        linear_vel = max_linear_vel;
      }
      setVelocity(*base_cmd, linear_vel, 0);
    }
    else if (cKey == 'x')
    {
      setVelocity(*base_cmd, 0, 0);
      printf("Exited! \n");
      //延迟1秒
      std::this_thread::sleep_for(std::chrono::seconds(1));
      return 0;
    }
    cmd_vel_pub->publish(*base_cmd);
  }

  rclcpp::shutdown();
  return 0;
}